#!/usr/bin/env python3
# encoding: utf-8
# @Author: Zecous
# @Date: 2025/07/18
import copy
import cv2
import numpy as np
import rospy
import signal
import time
import math
from sensor_msgs.msg import Image
from std_msgs.msg import String, Bool
from std_srvs.srv import Trigger, TriggerResponse
from cv_bridge import CvBridge
from ainex_sdk import misc, common
from ainex_example.color_common import Common

class LineFollowing(Common):
    def __init__(self, name):
        # 初始化头部位置
        self.head_pan_init = 500   # 左右舵机的初始值
        self.head_tilt_look_down = 280  # 低头位置
        
        # 初始化ROS节点
        rospy.init_node(name)
        self.name = name
        self.running = True
        self.active = True  # 用于外部控制启停
        
        super().__init__(name, self.head_pan_init, self.head_tilt_look_down)
        
        # 设置退出处理函数
        signal.signal(signal.SIGINT, self.shutdown)
        
        # Initialize CV Bridge
        self.bridge = CvBridge()
        
        # State variables
        self.current_image = None
        self.line_detected = False
        
        # 角度滤波相关变量
        self.angle_history = []  # 角度历史记录
        self.last_filtered_angle = 0.0  # 上一次滤波后的角度
        
        # 初始化运动参数
        self.init_movement_params()
        
        # 图像参数
        self.image_width = rospy.get_param('~image_width', 640)
        self.image_height = rospy.get_param('~image_height', 480)
        self.center_x = self.image_width // 2
        
        # 从ROS参数服务器加载配置
        self.load_config()
        
        # 面积过滤参数 - 从配置文件加载
        self.load_area_params()
        
        # 霍夫变换参数
        self.hough_threshold = 30       # 霍夫变换阈值（降低以检测更多线条）
        self.min_line_length = 20       # 最小线条长度（降低以适应各种角度的线段）
        self.max_line_gap = 15          # 最大线条间隙（增加以连接虚线段）
        
        # 虚线过滤参数
        self.max_line_length = 1000      # 最大线段长度，过滤连续长线（虚线应该是短段）
        self.enable_line_length_filter = True  # 是否启用线条长度过滤
        
        # 慢速前进参数
        self.slow_forward_ratio = 0.5   # 慢速前进速度比例（相对于正常速度，丢失线条时使用）
        
        # ROS通信接口
        self.setup_ros_interface()

        # 显示窗口控制
        self.update_display_config()
        
        # Subscribe to camera topic
        self.image_subscriber = rospy.Subscriber('/camera/image_raw', Image, self.image_callback)
        
        # 初始化机器人动作
        self.motion_manager.run_action('walk_ready')
        
        # 设置低头位置
        self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        # time.sleep(2)  # 等待舵机到位
        
        common.loginfo('Line following system initialized with ROS interface')
        print("Starting line following with white line detection...")
        
        # 测试角度滤波功能
        self.test_angle_filter()

    def update_display_config(self):
        """从参数服务器更新显示配置"""
        display_ns = '/system/display'
        main_flag = rospy.get_param(f'{display_ns}/show_line_following_window', None)
        if main_flag is None:
            main_flag = rospy.get_param(f'{display_ns}/show_main_window', True)
        self.display_main_window = bool(main_flag)

        mask_flag = rospy.get_param(f'{display_ns}/show_detection_masks', True)
        self.display_mask_window = self.display_main_window and bool(mask_flag)

    @staticmethod
    def _safe_destroy_window(window_name):
        try:
            cv2.destroyWindow(window_name)
        except cv2.error:
            pass
        except Exception:
            pass

    def load_config(self):
        """从ROS参数服务器加载配置"""
        try:
            # 从参数服务器获取颜色配置
            white_config = rospy.get_param('/image_processing/colors/white', {})
            self.white_lower = np.array(white_config.get('lower', [190, 0, 0]))
            self.white_upper = np.array(white_config.get('upper', [255, 255, 255]))
            
            # 从参数服务器获取舵机配置
            servo_config = rospy.get_param('/servo', {})
            self.head_pan_init = servo_config.get('head_pan_init', 500)
            self.head_tilt_look_down = servo_config.get('head_tilt_look_down', 250)
            
            # print(f"配置加载成功 - 白色范围: {self.white_lower} ~ {self.white_upper}")
            
            # 从参数服务器获取慢速前进参数
            line_following_config = rospy.get_param('/tasks/line_following', {})
            self.slow_forward_ratio = line_following_config.get('slow_forward_ratio', 0.5)
            # print(f"慢速前进速度比例: {self.slow_forward_ratio}")
            
        except Exception as e:
            print(f"配置加载失败: {e}")
            # 使用默认值
            self.slow_forward_ratio = 0.5

    def load_area_params(self):
        """从配置加载面积过滤参数"""
        try:
            line_following_config = rospy.get_param('/tasks/line_following', {})
            self.min_line_area = line_following_config.get('min_line_area', 300)
            self.max_line_area = line_following_config.get('max_line_area', 3000)
            self.max_area_ratio = line_following_config.get('max_area_ratio', 0.3)
            
            # 加载角度滤波参数
            angle_filter_config = line_following_config.get('angle_filter', {})
            self.max_angle_change = angle_filter_config.get('max_angle_change', 3.0)
            self.angle_filter_window = angle_filter_config.get('filter_window_size', 5)
            self.enable_smoothing = angle_filter_config.get('enable_smoothing', True)
            
            print(f"角度滤波参数加载成功 - 最大角度变化: {self.max_angle_change}°, 窗口大小: {self.angle_filter_window}, 平滑: {self.enable_smoothing}")
            
        except Exception as e:
            print(f"参数加载失败，使用默认值: {e}")
            # 使用默认配置
            self.min_line_area = 300
            self.max_line_area = 3000
            self.max_area_ratio = 0.3
            self.max_angle_change = 3.0
            self.angle_filter_window = 5
            self.enable_smoothing = True

    def setup_ros_interface(self):
        """设置ROS通信接口"""
        # 状态发布器
        self.status_pub = rospy.Publisher('/line_follow/status', String, queue_size=1)
        self.active_pub = rospy.Publisher('/line_follow/active', Bool, queue_size=1)
        
        # 控制服务
        self.start_service = rospy.Service('/line_follow/start', Trigger, self.start_service_callback)
        self.stop_service = rospy.Service('/line_follow/stop', Trigger, self.stop_service_callback)
        self.pause_service = rospy.Service('/line_follow/pause', Trigger, self.pause_service_callback)
        self.resume_service = rospy.Service('/line_follow/resume', Trigger, self.resume_service_callback)
        
        # 控制命令订阅
        self.control_sub = rospy.Subscriber('/line_follow/control', String, self.control_callback)

    def start_service_callback(self, req):
        """启动服务回调"""
        self.active = True
        self.running = True
        msg = String()
        msg.data = "started"
        self.status_pub.publish(msg)
        return TriggerResponse(success=True, message="Line following started")

    def stop_service_callback(self, req):
        """停止服务回调"""
        self.active = False
        self.gait_manager.stop()
        msg = String()
        msg.data = "stopped"
        self.status_pub.publish(msg)
        return TriggerResponse(success=True, message="Line following stopped")

    def pause_service_callback(self, req):
        """暂停服务回调"""
        self.active = False
        self.gait_manager.stop()
        msg = String()
        msg.data = "paused"
        self.status_pub.publish(msg)
        return TriggerResponse(success=True, message="Line following paused")

    def resume_service_callback(self, req):
        """恢复服务回调"""
        self.active = True
        msg = String()
        msg.data = "resumed"
        self.status_pub.publish(msg)
        return TriggerResponse(success=True, message="Line following resumed")

    def control_callback(self, msg):
        """控制命令回调"""
        command = msg.data.lower()
        if command == "start":
            self.active = True
            self.running = True
        elif command == "stop":
            self.active = False
            self.gait_manager.stop()
        elif command == "pause":
            self.active = False
            self.gait_manager.stop()
        elif command == "resume":
            self.active = True

    def publish_status(self, status):
        """发布状态"""
        msg = String()
        msg.data = status
        self.status_pub.publish(msg)
        
        active_msg = Bool()
        active_msg.data = self.active
        self.active_pub.publish(active_msg)

    def init_movement_params(self):
        """初始化运动参数"""
        self.calib_config = common.get_yaml_data('/home/ubuntu/ros_ws/src/ainex_example/config/calib.yaml')
        
        # 运动参数配置
        self.arm_swap = 60                   # 摆臂幅度（直行）
        self.turn_arm_swap = 52              # 摆臂幅度（转弯）略减弱，提高脚底抓地
        self.forward_step_distance = 0.016   # 直行默认步长
        self.turn_step_distance = 0.010      # 转弯默认步长，更专注转向
        self.turn_yaw_enter = rospy.get_param('~turn_yaw_enter', 4.5)
        self.turn_yaw_exit = rospy.get_param('~turn_yaw_exit', 2.8)

        # DSP参数（双足支撑期配置）
        self.forward_dsp_param = [280, 0.18, 0.025]
        self.turn_dsp_param = [250, 0.15, 0.030]

        # 步态参数（根据竞赛节点的稳定配置进行调整）
        self.walking_param = None
        self.turning_param = None
        self.apply_stable_gait_params()

        # 当前步长（会在控制逻辑中动态调整）
        self.step_distance = self.forward_step_distance
        self.yaw_bias = rospy.get_param('~yaw_bias', -0.03)
        self.turn_mode_active = False

    def apply_stable_gait_params(self):
        """应用比赛节点中稳定的直行与转弯步态参数"""
        base_param = self.gait_manager.get_gait_param()

        go_param = copy.deepcopy(base_param)
        go_param['angle_move_amplitude'] = -4.0
        go_param['step_height'] = 0.020
        go_param['z_swap_amplitude'] = 0.004
        go_param['init_roll_offset'] = -5.0
        go_param['init_pitch_offset'] = -4.0
        go_param['step_fb_ratio'] = 0.028
        go_param['init_y_offset'] = -0.004  # 给右腿更多内收，抑制左偏
        go_param['pelvis_offset'] = 2
        go_param['hip_pitch_offset'] = 5.5
        self.walking_param = go_param

        turn_param = copy.deepcopy(base_param)
        turn_param['angle_move_amplitude'] = -4.0
        turn_param['step_height'] = 0.022  # 增加转弯步高，提高转弯灵敏度
        turn_param['z_swap_amplitude'] = 0.006  # 增加摆幅
        turn_param['init_roll_offset'] = -5.0
        turn_param['init_pitch_offset'] = -4.0
        turn_param['step_fb_ratio'] = 0.020  # 略减转弯时的前进比，更专注转向
        turn_param['init_y_offset'] = 0
        turn_param['pelvis_offset'] = 4.0  # 增加转弯时的髋摆幅度
        turn_param['hip_pitch_offset'] = 3  # 减少前倾，增加稳定性
        self.turning_param = turn_param

    def image_callback(self, msg):
        """ROS图像回调函数"""
        try:
            self.current_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except Exception as e:
            rospy.logerr(f"Failed to convert image: {e}")

    def filter_white_areas_by_size(self, mask):
        """根据面积过滤白色区域，去除过大的区域"""
        # 计算最大允许面积
        image_area = self.image_width * self.image_height
        max_area_threshold = min(self.max_line_area, image_area * self.max_area_ratio)
        
        # 寻找轮廓
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        # 创建新的掩码
        filtered_mask = np.zeros_like(mask)
        valid_areas = []
        
        for contour in contours:
            area = cv2.contourArea(contour)
            
            # 只保留面积在合理范围内的区域
            if self.min_line_area <= area <= max_area_threshold:
                cv2.fillPoly(filtered_mask, [contour], 255)
                valid_areas.append({
                    'contour': contour,
                    'area': area,
                    'bbox': cv2.boundingRect(contour)
                })
        
        # print(f"Area filtering: {len(contours)} -> {len(valid_areas)} regions (min: {self.min_line_area}, max: {max_area_threshold:.0f})")
        
        return filtered_mask, valid_areas

    def detect_white_lines(self, image):
        """检测白色线条并找到最近的三条线拟合中心线"""
        if image is None:
            return None, None, None, None
        
        # 转换为LAB色彩空间
        rgb_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
        lab = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2LAB)
        
        # 调试：检查配置是否正确加载
        # print(f"DEBUG - 白色阈值: lower={self.white_lower}, upper={self.white_upper}")
        # print(f"DEBUG - 阈值类型: {type(self.white_lower)}, {type(self.white_upper)}")
        # print(f"DEBUG - 阈值形状: {self.white_lower.shape}, {self.white_upper.shape}")
        
        # 创建白色掩码
        mask = cv2.inRange(lab, self.white_lower, self.white_upper)
        
        # 屏蔽画面边缘10%的区域
        h, w = mask.shape
        # border_h = int(h * 0.1)  # 垂直边缘10%
        # border_w = int(w * 0.1)  # 水平边缘10%
        border_h = 0 # 垂直边缘10%
        border_w = 0 # 水平边缘10%
        
        # 创建边缘掩码（中心区域为255，边缘区域为0）
        edge_mask = np.zeros((h, w), dtype=np.uint8)
        edge_mask[border_h:h-border_h, border_w:w-border_w] = 255
        
        # 应用边缘掩码到白色检测掩码
        mask = cv2.bitwise_and(mask, edge_mask)
        
        # 调试：检查掩码结果
        white_pixels = cv2.countNonZero(mask)
        total_pixels = mask.shape[0] * mask.shape[1]
        # print(f"DEBUG - 掩码结果: {white_pixels}/{total_pixels} ({white_pixels/total_pixels*100:.1f}%)")
        
        # 形态学操作去除噪声
        kernel = np.ones((3, 3), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
        
        # 根据面积过滤白色区域
        filtered_mask, valid_areas = self.filter_white_areas_by_size(mask)
        
        # 使用霍夫直线变换检测线条
        lines = cv2.HoughLinesP(filtered_mask, 1, np.pi/180, 
                               threshold=self.hough_threshold, 
                               minLineLength=self.min_line_length, 
                               maxLineGap=self.max_line_gap)
        
        # 调试信息
        # if lines is not None:
        #     print(f"霍夫变换检测到 {len(lines)} 条线")
        # else:
        #     print("霍夫变换未检测到任何线条")
        
        if lines is not None:
            # 从下往上选择线条，确保间距大于100像素且不重合
            selected_lines = []
            filtered_long_lines = 0  # 记录被过滤的长线条数量
            bottom_center = (self.center_x, self.image_height)
            
            for line in lines:
                x1, y1, x2, y2 = line[0]
                # 计算线段中点
                mid_x = (x1 + x2) / 2
                mid_y = (y1 + y2) / 2
                
                # 计算线段长度
                line_length = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
                
                # 线条长度过滤（过滤过长的连续线条，保留虚线段）
                if self.enable_line_length_filter and line_length > self.max_line_length:
                    # print(f"过滤过长线条: 长度={line_length:.1f}px > {self.max_line_length}px")
                    filtered_long_lines += 1
                    continue
                
                # 计算到底部中心的距离
                distance = math.sqrt((mid_x - bottom_center[0])**2 + (mid_y - bottom_center[1])**2)
                
                # 符合长度要求的线条都加入
                can_add = True
                
                if can_add:
                    selected_lines.append({
                        'distance': distance,
                        'line': line[0],
                        'midpoint': (mid_x, mid_y),
                        'length': line_length,
                        'y_position': mid_y  # 添加y位置用于排序
                    })
            
            # 按y位置从下往上排序（y值越大越靠下）
            selected_lines.sort(key=lambda x: x['y_position'], reverse=True)
            closest_lines = selected_lines
            
            # 调试信息：显示过滤统计
            if filtered_long_lines > 0:
                print(f"线条过滤: 检测到{len(lines)}条 -> 过滤{filtered_long_lines}条长线 -> 保留{len(closest_lines)}条虚线段")
            # 可选：显示保留线条的长度统计
            # if closest_lines:
            #     lengths = [line['length'] for line in closest_lines]
            #     print(f"保留线条长度: {min(lengths):.1f}-{max(lengths):.1f}px (平均{sum(lengths)/len(lengths):.1f}px)")
            
            # 为选定的线条添加面积信息
            for line_info in closest_lines:
                # 找到对应的有效区域
                line_midpoint = line_info['midpoint']
                for area_info in valid_areas:
                    x, y, w, h = area_info['bbox']
                    if x <= line_midpoint[0] <= x + w and y <= line_midpoint[1] <= y + h:
                        line_info['area'] = area_info['area']
                        break
                else:
                    # 如果没有找到对应的区域，设置为0
                    line_info['area'] = 0
            
            # print(f"线条处理: {len(lines)} 条原始线 -> {len(closest_lines)} 条最终线")
            
            if len(closest_lines) >= 1:  # 至少需要一条线
                # 提取中点坐标
                points = [line_info['midpoint'] for line_info in closest_lines]
                
                # 拟合直线
                fitted_line = self.fit_line_to_points(points)
                return fitted_line, filtered_mask, closest_lines, valid_areas
        
        return None, filtered_mask, None, valid_areas

    def fit_line_to_points(self, points):
        """对点进行直线拟合"""
        if len(points) < 2:
            return None
        
        # 转换为numpy数组
        points_array = np.array(points, dtype=np.float32)
        
        # print(f"DEBUG - 拟合点坐标: {points}")
        
        # 使用cv2.fitLine进行直线拟合
        line_params = cv2.fitLine(points_array, cv2.DIST_L2, 0, 0.01, 0.01)
        
        # 提取直线参数
        vx, vy, x0, y0 = line_params.flatten()
        
        # print(f"DEBUG - CV2方向向量: vx={vx:.4f}, vy={vy:.4f}")
        
        # 计算直线在图像中部和上部的点
        y_middle = self.image_height / 2 + 100 # 中部
        y_top = 50  # 上部附近
        
        # 计算对应的x坐标
        if vy != 0:
            x_middle = x0 + vx * (y_middle - y0) / vy
            x_top = x0 + vx * (y_top - y0) / vy
            
            raw_angle = math.atan2(vx, vy) * 180 / math.pi
            # print(f"DEBUG - 原始CV2角度: {raw_angle:.2f}度")
            
            # 使用端点位移计算角度
            dx = x_top - x_middle        # 水平位移
            dy = y_middle - y_top        # 垂直位移（向上为正）
            
            # print(f"DEBUG - 端点位移: dx={dx:.1f}, dy={dy:.1f}")
            
            if dy > 0:  # 确保有向前的分量
                angle = math.atan2(dx, dy) * 180 / math.pi
                # print(f"DEBUG - 计算过程: atan2({dx:.1f}, {dy:.1f}) = {angle:.2f}度")
            else:
                angle = 0
                # print("DEBUG - 警告: 没有向前分量，设置角度为0")
            
            # 验证计算
            if len(points) >= 2:
                points_sorted = sorted(points, key=lambda p: p[1])  # 按y坐标升序
                top_pt = points_sorted[0]      # 最上面的点
                bottom_pt = points_sorted[-1]  # 最下面的点
                
                simple_dx = top_pt[0] - bottom_pt[0]
                simple_dy = bottom_pt[1] - top_pt[1]  # 向上为正
                
                if simple_dy > 0:
                    simple_angle = math.atan2(simple_dx, simple_dy) * 180 / math.pi
                    # print(f"DEBUG - 简单验证角度: {simple_angle:.2f}度 (用点{top_pt} -> {bottom_pt})")
            
            # print(f"DEBUG - 最终道路偏差角度: {angle:.2f}度")
            
            return {
                'middle_point': (int(x_middle), int(y_middle)),
                'top_point': (int(x_top), int(y_top)),
                'angle': angle,  # 修复后的角度
                'raw_cv2_angle': raw_angle,  # 保留原始CV2角度用于对比
                'displacement': (dx, dy)  # 保存位移信息
            }
        
        return None

    def filter_angle(self, raw_angle):
        """
        对角度进行滤波，限制相邻帧间的角度变化
        
        Args:
            raw_angle (float): 原始检测到的角度
            
        Returns:
            float: 滤波后的角度
        """
        if raw_angle is None:
            return self.last_filtered_angle
        
        # 如果这是第一次检测到角度，直接使用
        if len(self.angle_history) == 0:
            filtered_angle = raw_angle
        else:
            # 计算与上一次滤波角度的差值
            angle_diff = raw_angle - self.last_filtered_angle
            
            # 限制角度变化幅度
            if abs(angle_diff) > self.max_angle_change:
                # 如果变化过大，则逐步调整
                if angle_diff > 0:
                    filtered_angle = self.last_filtered_angle + self.max_angle_change
                else:
                    filtered_angle = self.last_filtered_angle - self.max_angle_change
            else:
                # 如果变化在合理范围内，使用原始角度
                filtered_angle = raw_angle
        
        # 更新角度历史记录
        self.angle_history.append(filtered_angle)
        
        # 保持历史记录窗口大小
        if len(self.angle_history) > self.angle_filter_window:
            self.angle_history.pop(0)
        
        # 使用滑动窗口平均进一步平滑（可配置）
        if self.enable_smoothing and len(self.angle_history) >= 3:
            # 取最近3个角度的加权平均，给当前角度更高权重
            weights = [0.1, 0.3, 0.6]  # 历史权重：越近的权重越大
            recent_angles = self.angle_history[-3:]
            smoothed_angle = sum(angle * weight for angle, weight in zip(recent_angles, weights))
            self.last_filtered_angle = smoothed_angle
        else:
            self.last_filtered_angle = filtered_angle
        
        # 打印调试信息（可选，调试时启用）
        # print(f"角度滤波: 原始={raw_angle:.2f}° -> 限制后={filtered_angle:.2f}° -> 平滑后={self.last_filtered_angle:.2f}°")
        
        return self.last_filtered_angle

    def test_angle_filter(self):
        """测试角度滤波功能"""
        print("开始测试角度滤波功能...")
        
        # 模拟一系列角度值，包括突然变化
        test_angles = [0, 2, 1, 8, 10, 5, 3, 2, 15, 12, 10, 8, 6]
        
        for i, angle in enumerate(test_angles):
            filtered = self.filter_angle(angle)
            print(f"步骤 {i+1}: 输入角度={angle}°, 滤波后={filtered:.2f}°")
        
        print("角度滤波测试完成\n")

    def control_robot_movement(self, fitted_line):
        """根据拟合的直线控制机器人移动"""
        if not self.active:  # 如果被暂停，不执行移动
            return
            
        if fitted_line is None:
            # print("No line detected, continuing slow forward...")
            # 没有检测到线条时，使用滤波器保持角度稳定
            filtered_angle = self.filter_angle(None)  # 传入None，保持上一次的角度
            # print(f"没有线条时使用滤波角度: {filtered_angle:.2f}°")
            
            # 保持慢速前进
            try:
                self.gait_manager.set_step(
                    self.forward_dsp_param,               # DSP参数
                    self.forward_step_distance * self.slow_forward_ratio,  # 慢速前进
                    0,                           # y方向步长（不侧移）
                    0,                           # yaw角度（直行）
                    self.walking_param,          # 步态参数（稳定直行）
                    self.arm_swap,               # 摆臂幅度
                    0                            # 连续移动
                )
                self.publish_status("no_line_slow_forward")
            except Exception as e:
                print(f"Error during slow forward movement: {e}")
                rospy.logerr(f"Slow forward movement error: {e}")
                self.publish_status("error")
            return
        
        # 获取直线角度和中点
        raw_angle = fitted_line['angle']
        middle_x, middle_y = fitted_line['middle_point']
        
        # 应用角度滤波
        angle = self.filter_angle(raw_angle)
        
        # 计算与图像中心的偏差
        deviation_x = middle_x - self.center_x
        
        # 简化调试输出 - 只在需要时启用
        # print(f"Line angle: 原始={raw_angle:.2f}° 滤波后={angle:.2f}°, middle point: ({middle_x}, {middle_y}), deviation: {deviation_x}")
        
        # 根据角度和偏差计算转向
        yaw_output = 0
        # print("Deviation = ",deviation_x) 
        # 基于偏差的左右调整
        if abs(deviation_x) > 10:  # 偏差阈值
            if deviation_x > 0:  # 线偏右，机器人需要右转
                yaw_output += -(abs(deviation_x) / 14)
            else:  # 线偏左，机器人需要左转
                yaw_output += abs(deviation_x / 14)
        
        # 基于角度的方向调整
        if abs(angle) > 10:  # 角度阈值
            if angle > 0:  # 线向右倾斜，机器人需要右转
                yaw_output += -(abs(angle) / 11)
            else:  # 线向左倾斜，机器人需要左转
                yaw_output += abs(angle) / 11
        
        # 当线条角度接近45度时，增加转向幅度（仅在转向不足时临时启用）
        # if abs(angle) > 30:  # 接近45度时
            # if angle > 0:  # 线向右倾斜，机器人需要右转
                # yaw_output += -2
            # else:  # 线向左倾斜，机器人需要左转
                # yaw_output += 2 
        
        raw_yaw = yaw_output

        # 根据滞回阈值决定是否进入转弯模式
        if self.turn_mode_active:
            if abs(raw_yaw) <= self.turn_yaw_exit:
                self.turn_mode_active = False
        else:
            if abs(raw_yaw) >= self.turn_yaw_enter:
                self.turn_mode_active = True

        if not self.turn_mode_active:
            yaw_output += self.yaw_bias

        use_turn_profile = self.turn_mode_active
        yaw_limit = 11.0 if use_turn_profile else 8.0
        yaw_output = max(-yaw_limit, min(yaw_limit, yaw_output))

        step_distance = self.forward_step_distance
        dsp_param = self.forward_dsp_param
        walking_param = self.walking_param
        arm_swap = self.arm_swap

        if use_turn_profile:
            step_distance = self.turn_step_distance
            dsp_param = self.turn_dsp_param
            walking_param = self.turning_param
            arm_swap = self.turn_arm_swap

        command_yaw = yaw_output
        if use_turn_profile:
            command_yaw *= 1.1  # 转弯时增加幅度，加快转弯速度

        if abs(command_yaw) >= 10:
            step_distance = min(step_distance, 0.004)  # 大转向时保持一定前进速度
            dsp_param = self.turn_dsp_param
        elif use_turn_profile:
            step_distance = min(step_distance, 0.006)  # 转弯时保持更多前进速度

        self.step_distance = step_distance

        try:
            # 执行移动
            self.gait_manager.set_step(
                dsp_param,           # DSP参数
                step_distance,       # x方向步长（前进）
                0,                   # y方向步长（不侧移）
                int(max(-12, min(12, command_yaw))),     # yaw角度（转向）
                walking_param,       # 步态参数
                arm_swap,            # 摆臂幅度
                0                    # 连续移动
            )
            
            print(f"Moving forward with yaw: {yaw_output}")  # 简化输出
            self.publish_status("following")
            
        except Exception as e:
            print(f"Error during robot movement: {e}")
            rospy.logerr(f"Movement error: {e}")
            self.publish_status("error")

    def draw_line_detections(self, image, closest_lines, valid_areas, fitted_line):
        """绘制线条检测结果"""
        display_image = image.copy()
        
        # 1. 只显示前3条线条
        if closest_lines is not None:
            max_display_lines = min(3, len(closest_lines))  # 最多显示3条线
            for i in range(max_display_lines):
                line_info = closest_lines[i]
                line = line_info['line']
                x1, y1, x2, y2 = line
                
                # 用不同颜色标识线条优先级，但颜色更淡
                if i == 0:  # 最重要的线条用红色
                    color = (0, 0, 200)
                    thickness = 2
                elif i == 1:  # 第二重要用橙色
                    color = (0, 140, 200)
                    thickness = 1
                else:  # 第三重要用黄色
                    color = (0, 200, 200)
                    thickness = 1
                
                cv2.line(display_image, (x1, y1), (x2, y2), color, thickness)
                
                # 只在最重要的线条上标记中点
                if i == 0:
                    mid_x, mid_y = line_info['midpoint']
                    cv2.circle(display_image, (int(mid_x), int(mid_y)), 3, color, -1)
        
        # 2. 绘制拟合的中心线
        if fitted_line is not None:
            middle_point = fitted_line['middle_point']
            top_point = fitted_line['top_point']
            
            # 用粗蓝色线绘制拟合结果
            cv2.line(display_image, middle_point, top_point, (255, 0, 0), 4)
            cv2.circle(display_image, middle_point, 6, (255, 0, 0), -1)
            cv2.circle(display_image, top_point, 4, (255, 0, 0), -1)
            
            angle = fitted_line['angle']
            deviation = middle_point[0] - self.center_x
            cv2.putText(display_image, f"Angle: {angle:.1f}deg", (10, 30), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
            cv2.putText(display_image, f"Deviation: {deviation}px", (10, 50), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
        
        # 3. 绘制图像中心线
        cv2.line(
            display_image,
            (self.center_x, 0),
            (self.center_x, self.image_height),
            (100, 100, 100),  
            1
        )
        
        # 4. 统计信息显示
        if closest_lines:
            total_lines = len(closest_lines)
            displayed_lines = min(3, total_lines)
            # 显示线条长度范围
            lengths = [line['length'] for line in closest_lines]
            min_len, max_len = min(lengths), max(lengths)
            status_text = f"Lines: {displayed_lines}/{total_lines} (L:{min_len:.0f}-{max_len:.0f}px)"
        else:
            status_text = "Lines: 0/0"
        
        cv2.putText(display_image, status_text, (10, self.image_height - 35), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.4, (200, 200, 200), 1)
        
        # 显示过滤设置
        filter_text = f"Filter: MaxLen={self.max_line_length}px {'ON' if self.enable_line_length_filter else 'OFF'}"
        cv2.putText(display_image, filter_text, (10, self.image_height - 15), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.4, (150, 255, 150) if self.enable_line_length_filter else (150, 150, 150), 1)
        
        return display_image

    def set_servo(self, pan, tilt):
        """设置舵机位置"""
        try:
            # 使用motion_manager控制舵机，时间200ms，舵机23为左右，舵机24为上下
            self.motion_manager.set_servos_position(200, [[23, pan], [24, tilt]])
        except Exception as e:
            rospy.logerr(f"Servo control error: {e}")

    def shutdown(self, signum, frame):
        """节点关闭回调函数"""
        with self.lock:
            self.motion_manager.run_action('stand')
            self.running = False 
            self.active = False
            self.publish_status("shutdown")
            common.loginfo('%s shutdown' % self.name)

    def run(self):
        """主循环函数"""
        print("Waiting for camera image...")
        
        # 等待图像数据
        while self.current_image is None and self.running:
            rospy.sleep(0.1)
        
        print("Camera ready, starting line following...")
        self.publish_status("ready")
        
        # 窗口显示状态
        windows_created = False
        mask_window_created = False
        
        while self.running and not rospy.is_shutdown():
            try:
                # 支持运行时更新显示配置
                self.update_display_config()
                if not self.display_main_window and windows_created:
                    self._safe_destroy_window('Line Following - Enhanced Detection')
                    windows_created = False
                if (not self.display_mask_window or not self.display_main_window) and mask_window_created:
                    self._safe_destroy_window('Filtered White Mask')
                    mask_window_created = False

                if self.current_image is not None and self.active:
                    # 检测白色线条
                    fitted_line, filtered_mask, closest_lines, valid_areas = self.detect_white_lines(self.current_image)
                    
                    # 创建显示图像
                    display_image = self.draw_line_detections(self.current_image, closest_lines, valid_areas, fitted_line)
                    
                    # 控制机器人移动
                    if fitted_line is not None:
                        cv2.putText(display_image, "LINE FOLLOWING ACTIVE", (10, 90), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        self.control_robot_movement(fitted_line)
                    else:
                        cv2.putText(display_image, "NO LINE - SLOW FORWARD", (10, 90), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 165, 255), 2)  # 橙色表示慢速前进
                        if self.active:
                            self.control_robot_movement(fitted_line)  # 传入None，触发慢速前进
                    
                    # 显示图像（只在激活时）
                    if self.display_main_window:
                        cv2.imshow('Line Following - Enhanced Detection', display_image)
                        windows_created = True
                        if filtered_mask is not None and self.display_mask_window:
                            cv2.imshow('Filtered White Mask', filtered_mask)
                            mask_window_created = True
                        elif mask_window_created and (filtered_mask is None or not self.display_mask_window):
                            self._safe_destroy_window('Filtered White Mask')
                            mask_window_created = False

                        # 检查退出键
                        key = cv2.waitKey(30) & 0xFF
                        if key == 27:  # ESC键
                            break
                    else:
                        rospy.sleep(0.05)
                        
                elif not self.active and windows_created:
                    # 任务未激活时关闭窗口
                    self._safe_destroy_window('Line Following - Enhanced Detection')
                    if mask_window_created:
                        self._safe_destroy_window('Filtered White Mask')
                        mask_window_created = False
                    windows_created = False
                    rospy.sleep(0.1)
                else:
                    rospy.sleep(0.1)
                    
            except KeyboardInterrupt:
                break
            except Exception as e:
                print(f"Error in main loop: {e}")
                rospy.sleep(0.1)
        
        # 退出前动作
        self.gait_manager.stop()
        self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        self.publish_status("stopped")
        if windows_created:
            self._safe_destroy_window('Line Following - Enhanced Detection')
        if mask_window_created:
            self._safe_destroy_window('Filtered White Mask')
        rospy.signal_shutdown('shutdown')

if __name__ == '__main__':
    try:
        line_follower = LineFollowing('line_following')
        line_follower.run()
    except Exception as e:
        print(f"Failed to start line follower: {e}")
        rospy.logerr(f"Line follower initialization error: {e}")
